﻿#include "plccomm.h"
#include <QDebug>
#include<QTimer>
PlcComm::PlcComm(QObject *parent) : QThread(parent)
{
    m_stopped = false;
    m_grabImage[0] = false;
    m_grabImage[1] = false;
    m_matchModel[0] = false;
    m_matchModel[1] = false;
    m_loopCnt = 0;
}
void PlcComm::displayError(QAbstractSocket::SocketError socketError)
{
    switch (socketError) {
    case QAbstractSocket::RemoteHostClosedError:
        qDebug() << "QAbstractSocket::RemoteHostClosedError";
        break;
    case QAbstractSocket::HostNotFoundError:
        qDebug() << "QAbstractSocket::HostNotFoundError";
        break;
    case QAbstractSocket::ConnectionRefusedError:
        qDebug() << "QAbstractSocket::ConnectionRefusedError";
        break;
    default:
        qDebug() <<  tr("The following error occurred: %1.").arg(m_socket->errorString());
    }
}
bool PlcComm::connectToRobot(QString ip, int port)
{
    m_ip = ip;
    m_port = port;
    start();

    return true;
}

void PlcComm::setDisconnected()
{
    m_connected = false;
}

void PlcComm::reConnect()
{
    if(!m_ip.isEmpty() && !m_connected)
    {
        m_socket->connectToHost(m_ip, m_port);
    }
}

void PlcComm::onDisconnected()
{
    emit plcDisconnected();
    if(!m_ip.isEmpty())
    {
        QTimer::singleShot(10000, this, &PlcComm::setDisconnected);
    }
}

bool PlcComm::setData(int robot, double x[2], double y[2], double theta[2], int N)
{
    m_data[robot].clear();
    if(N == 0) return false;
    PositionCorrect pc = {{0,0,},{0,0,},{0,0}};
    pc.x[0] = x[0];
    pc.y[0] = y[0];
    pc.theta[0] = theta[0];

    //if(N == 2)
    {
        pc.x[1] = x[1];
        pc.y[1] = y[1];
        pc.theta[1] = theta[1];
    }
    m_data[robot].enqueue(pc);
    return true;
}

void PlcComm::SetGrabImageSuccess(int robot)
{
    m_grabImage[robot] = true;
}

void PlcComm::SetMatchModelFaild(int robot)
{
    m_matchModel[robot] = true;
}

void PlcComm::updateRobotData(int robot)
{
    Melsec_Comm_3E_BINARY melsec;
    uint8_t code = 0xA8;
    uint32_t timeout = 5000;

    if(!m_data[robot].isEmpty())
    {
        PositionCorrect pc = {{0,0,},{0,0,},{0,0}};
        pc = m_data[robot].dequeue();
        pc.theta[0] = 0;
        pc.theta[1] = 0;

        QVector<uint16_t> data;
        data.append((uint16_t)(pc.x[0] * 100));
        data.append((uint16_t)(pc.y[0] * 100));
        data.append((uint16_t)(pc.theta[0] * 100));
        data.append((uint16_t)(pc.x[1] * 100));
        data.append((uint16_t)(pc.y[1] * 100));
        data.append((uint16_t)(pc.theta[1] * 100));

        if (robot == LEFT_ROBOT)
        {
            melsec.writeBytes(m_socket, timeout, code, 30023, data);
        }

        if (robot == RIGHT_ROBOT)
        {
            melsec.writeBytes(m_socket, timeout, code, 30033, data);
        }

        QString loffset = QString("%1, %2, %3\r").arg(pc.x[0], 0,'f', 3).arg(pc.y[0], 0,'f', 3).arg(pc.theta[0], 0,'f', 3);
        QString roffset = QString("%1, %2, %3\r").arg(pc.x[1], 0,'f', 3).arg(pc.y[1], 0,'f', 3).arg(pc.theta[1], 0,'f', 3);
        qDebug() << "Send to Robot :" << loffset << roffset << " , robot = " << robot;

        emit plcDataUpload(robot);
    }
}

void PlcComm::onConnected()
{
    m_connected = true;
    emit plcConnected();
}

void PlcComm::run()
{
    m_socket = new QTcpSocket(this);

    //m_socket->setSocketOption(QAbstractSocket::SocketOption::LowDelayOption, sockettimeout);
    m_stopped = false;
    m_connected = false;
    connect(m_socket, &QTcpSocket::connected, this, &PlcComm::onConnected);
    connect(m_socket, &QTcpSocket::disconnected, this, &PlcComm::onDisconnected);

    while (!m_stopped)
    {
        reConnect();
        handleOffset();
        handleTriggerImage();
        updateGrabImageResult();
        updateMatchModelResult();
        updateRobotData(LEFT_ROBOT);
        updateRobotData(RIGHT_ROBOT);

        QThread::msleep(10);
    }

    m_stopped = false;
}

void PlcComm::handleOffset()
{
    m_loopCnt++;
    if (m_loopCnt < 30)
    {
        return;
    }

    m_loopCnt = 0;
    Melsec_Comm_3E_BINARY melsec;
    uint8_t code = 0xA8;
    uint32_t timeout = 5000;

    QVector<int16_t> left = melsec.readInt16(m_socket, timeout, code, 30020, 1);
    if (left.length() == 0)
        return;

    QVector<int16_t> right = melsec.readInt16(m_socket, timeout, code, 30030, 1);
    if (right.length() == 0)
        return;
    if (left[0] == 0)
        emit offsetDisable(LEFT_ROBOT);;
    if (right[0] == 0)
        emit offsetDisable(RIGHT_ROBOT);;
//    QBitArray ofs = melsec.readBits(m_socket, timeout, code, 10010, 1);
//    if (ofs.size() == 0)
//        return;

    //if (ofs[0])
}

void PlcComm::handleTriggerImage()
{
   Melsec_Comm_3E_BINARY melsec;
   uint8_t code = 0xA8;
   uint32_t timeout = 5000;

   QVector<int16_t> tgr = melsec.readInt16(m_socket, timeout, code, 30000, 2);
   if (tgr.length() == 0)
       return;

   //  左机器人
   if (tgr[LEFT_ROBOT] == 1)
   {
//        QByteArray left_id = melsec.readString(m_socket, timeout, code, 30050, 10);
//        if (left_id.size() == 0)
//            return;
//        QByteArray right_id = melsec.readString(m_socket, timeout, code, 30060, 10);
//        if (right_id.size() == 0)
//            return;
//        QByteArray recipe = melsec.readString(m_socket, timeout, code, 30091, 3);
//        if (recipe.size() == 0)
//            return;

        QByteArray productinfo = melsec.readString(m_socket, timeout, code, 30050, 20);
        if (productinfo.size() == 0)
            return;
        QString id_01 = QString(productinfo.mid(0,20)).trimmed();
        QString id_02 = QString(productinfo.mid(20,20)).trimmed();
        QByteArray recipe = melsec.readString(m_socket, timeout, code, 30091, 3);
        if (recipe.size() == 0)
            return;

       //QString num = melsec.readString(m_socket, timeout, code, 30094, 1);
       //recipe += num;
       //QString recipe = recipestr.left(recipestr.indexOf(' '));

        //emit trigger(LEFT_ROBOT,QString(recipe).trimmed(), left_id, right_id);
        emit trigger(LEFT_ROBOT,QString(recipe).trimmed(), id_02, id_01);
        melsec.writeBytes(m_socket, timeout, code, 30000, QVector<uint16_t>(1, 0));
   }
    // 右机器人
   if (tgr[RIGHT_ROBOT] == 1)
   {
//        QByteArray left_id = melsec.readString(m_socket, timeout, code, 30070, 10);
//        if (left_id.size() == 0)
//            return;
//        QByteArray right_id = melsec.readString(m_socket, timeout, code, 30080, 10);
//        if (right_id.size() == 0)
//            return;
//        QByteArray recipe = melsec.readString(m_socket, timeout, code, 30091, 3);
//        if (recipe.size() == 0)
//            return;

       QByteArray productinfo = melsec.readString(m_socket, timeout, code, 30070, 20);
       if (productinfo.size() == 0)
           return;
       QString id_01 = QString(productinfo.mid(0,20)).trimmed();
       QString id_02 = QString(productinfo.mid(20,20)).trimmed();
       //QString recipe = QString(productinfo.mid(42,6));
       QByteArray recipe = melsec.readString(m_socket, timeout, code, 30091, 3);
       if (recipe.size() == 0)
               return;


        //QString recipe = recipestr.left(recipestr.indexOf(' '));
        //QString num = melsec.readString(m_socket, timeout, code, 30094, 1);
        //recipe += num;

        emit trigger(RIGHT_ROBOT, QString(recipe).trimmed(), id_02, id_01);
        melsec.writeBytes(m_socket, timeout, code, 30001, QVector<uint16_t>(1, 0));
   }
}

void PlcComm::updateGrabImageResult()
{
     Melsec_Comm_3E_BINARY melsec;
     uint8_t code = 0xA8;
     uint32_t timeout = 5000;

    if (m_grabImage[LEFT_ROBOT])
    {
        uint16_t iRet = melsec.writeBytes(m_socket, timeout, code, 30021,  QVector<uint16_t>(1, 1));
        if (iRet != 0)
            return;
        m_grabImage[LEFT_ROBOT] = false;
    }

    if (m_grabImage[RIGHT_ROBOT])
    {
        uint16_t iRet = melsec.writeBytes(m_socket, timeout, code, 30031,  QVector<uint16_t>(1, 1));
        if (iRet != 0)
            return;
        m_grabImage[RIGHT_ROBOT] = false;
    }
}

void PlcComm::updateMatchModelResult()
{
     Melsec_Comm_3E_BINARY melsec;
     uint8_t code = 0xA8;
     uint32_t timeout = 5000;

    if (m_matchModel[LEFT_ROBOT])
    {
        uint16_t iRet = melsec.writeBytes(m_socket, timeout, code, 30040,  QVector<uint16_t>(1, 1));
        if (iRet != 0)
            return;
        m_matchModel[LEFT_ROBOT] = false;
    }

    if (m_matchModel[RIGHT_ROBOT])
    {
        uint16_t iRet = melsec.writeBytes(m_socket, timeout, code, 30042,  QVector<uint16_t>(1, 1));
        if (iRet != 0)
            return;
        m_matchModel[RIGHT_ROBOT] = false;
    }
}

PlcComm::~PlcComm()
{
    m_stopped = true;
    m_socket->disconnectFromHost();
    m_socket->waitForDisconnected();
    delete m_socket;
}
